function [ FCN, RSa, rl, feh ] = curvemat( CNE, h, R0, f ) %#codegen TODO:根据《应用导航算法工程基础》更新算法
%CURVEMAT 计算曲率矩阵
%
% Tips:
% # E系EUGw，初始N系ENU（可以为非地理坐标系）
%
% # Input Arguments:
% # CNE: 3*3矩阵，位置矩阵
% # h: 标量，高度，单位m
% # R0: 标量，地球长半轴，单位m
% # f: 标量，地球扁率（REF1中也称为ellipticity，记为e），无单位
%
% Output Arguments:
% # FCN: 3*3矩阵，N系下的曲率矩阵
% # RSa: 标量，修改的到地球表面位置的经向距离，单位m
% # rl: 标量，当地导航点在纬度方向上的曲率半径，单位m
% # feh: 标量，参考文献1 5.3-18式
%
% References:
% # Paul G. Savage. Strapdown Analytics[M]. Maple Plain: Strapdown Associates, Inc., 2000: 5.1-5.3节

% earth polar axis component of geodetic vertical unit vector
uUpYE = CNE(2, 3); % Eq. 5.3-16

% modified radial distance to earth surface location
tmp = sqrt(1+(uUpYE^2)*((1-f)^2-1)); % 引入tmp为使R0为Inf时不奇异
RSa = R0 / tmp; % Eq. 5.1-10

% local earth surface point radius of curvature in latitude direction
rls = ((1-f)^2) * R0 / (tmp^3); % Eq. 5.2.4-25

% local navigation point radius of curvature in latitude direction
rl = rls + h; % Eq. 5.2.4-37

% N frame curvature matrix
fe = ((1-f)^2-1) / (1+(CNE(2, 3)^2)*((1-f)^2-1));
fh = 1 / (1+h/RSa);
feh = fe * fh;
FC11 = (1 + (CNE(2, 1)^2)*feh) / rl;
FC12 = CNE(2, 1) * CNE(2, 2) * feh / rl;
FC21 = FC12;
FC22 = (1 + (CNE(2, 2)^2)*feh) / rl;
FCN = [FC11 FC12 0; FC21 FC22 0; 0 0 0]; % Eq. 5.3-18
end